import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    package_name = 'robot_model'
    urdf_name = "robot.urdf"

    # 获取功能包路径
    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_path = os.path.join(pkg_share, 'urdf', urdf_name)
    
    # 检查文件是否存在
    if not os.path.exists(urdf_path):
        raise FileNotFoundError(f"URDF file not found at {urdf_path}")

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_path]
    )

    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        arguments=[urdf_path]
    )

    rviz_config_path = os.path.join(
        get_package_share_directory(package_name),
        'rviz',
        'model.rviz'
    )

    rviz = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        #arguments=[   # 在第一次运行后，保存配置后，指定配置文件的路径
        #    '-d', rviz_config_path
        #],
        output='screen'
    )

    return LaunchDescription([
        robot_state_publisher,
        joint_state_publisher,
        rviz
    ])
